function q_new = interpolate(q, index, interpo_num)

t = 1:length(q);

i1 = index-interpo_num; i2 = index-interpo_num + 1; i3 = index-interpo_num + 2; i4 = index+interpo_num - 2; i5 = index + interpo_num - 1; i6 = index+interpo_num;
t1 = t(i1); t2 = t(i2); t3 = t(i3); t4 = t(i4); t5 = t(i5); t6 = t(i6);
point1 = q(i1);
point2 = q(i2);
point3 = q(i3);

point4 = q(i4);
point5 = q(i5);
point6 = q(i6);

Point = [point1; point2; point3; point4; point5; point6];
Y = [1, t1, t1^2, t1^3, t1^4, t1^5;
    1, t2, t2^2, t2^3, t2^4, t2^5;
    1, t3, t3^2, t3^3, t3^4, t3^5;
    1, t4, t4^2, t4^3, t4^4, t4^5;
    1, t5, t5^2, t5^3, t5^4, t5^5;
    1, t6, t6^2, t6^3, t6^4, t6^5];
A = pinv(Y) * Point;
q_new = zeros(1, length(t));
for i = 1:length(t)
    if i < i1 || i > i6
        q_new(i) = q(i);
    else
        q_new(i) = [1, t(i), t(i)^2, t(i)^3, t(i)^4, t(i)^5] * A;
    end
end